/*
 *  acpFilter.cpp
 *  macx_aTemperature
 *
 *  Created by Acroname on 7/27/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "acpFilter.h"

/////////////////////////////////////////////////////////////////////

float
acpMovingAverageFilter::update(const float input) {
  float result = 0.0f;
  
  if (m_index == m_nSamples) m_index = 0;
  m_fValues[m_index++] = input;
  
  for (int i = 0; i< m_nSamples; i++)
    result += m_fValues[i];
  
  return (result / (float)m_nSamples);
}


/////////////////////////////////////////////////////////////////////

void
acpKalmanFilter::init(const float a, const float b, const float h) {
  // Call the origional init function to set other values
  init();
  
  // Set the new gain values
  m_a = a;
  m_h = h;
  m_b = b; // not used
}

/////////////////////////////////////////////////////////////////////


void
acpKalmanFilter::init(void) {
  
  // Define the gain constants for the system
  m_a = 1.0f;
  m_h = 3.0f;
  m_b = 0.0f; // not used
  
  // Set up initial guesses
  m_x = 0.0f;
  m_xaposteriori = 0.0f;
  m_paposteriori = 0.0f;
  
  // Define the noise
  m_w = 0.0f; // system noise
  m_v = 0.0f; // signal noise
  m_Q = 0.01f; // noise covariance
  m_R = 2.0f; // noise covariance
  
}


/////////////////////////////////////////////////////////////////////
// Discrete scalar Kalman filter
// http://www.swarthmore.edu/NatSci/echeeve1/Ref/Kalman/ScalarKalman.html
// http://www.swarthmore.edu/NatSci/echeeve1/Ref/Kalman/ScalarMatlab.html

float
acpKalmanFilter::update(const float input) {
  
  // Calculate the state based on input (with optional noise)
  m_x = m_a * input + m_w;
  
  // Calculate the output
  m_z = m_h * m_x + m_v;
  
  // Calculate the predictor equations
  m_xapriori = m_a * m_xaposteriori;
  m_residual = m_z - (m_h * m_xapriori);
  m_papriori = m_a * m_a * m_paposteriori + m_Q;
  
  // Corrector equations
  m_k = (m_h * m_papriori) / (m_h * m_h * m_papriori + m_R);
  m_paposteriori = m_papriori * (1 - m_h*m_k);
  m_xaposteriori = m_xapriori + m_k*m_residual;
  
  return m_xaposteriori;
}


